#include "positionmixermanager.h"
#include <QDebug>

PositionMixerManager::PositionMixerManager(double deltaTimeIn)
{
    deltaTime=deltaTimeIn;
}

int PositionMixerManager::reset()
{
    sumDistance=0;
    timeDuration=0;
    currentTransitionTimeAccumued=0;
    isTrajFinished=true;
}

int PositionMixerManager::addCeaseMotion(Frame startFrameIn, Frame endFrameIn, double startLineVelocity)
{
    startLineVelocity=fabs(startLineVelocity);
    startFrame=startFrameIn;
    endFrame=endFrameIn;
    sumDistance=sqrt((startFrameIn.p[0]-endFrame.p[0])*(startFrameIn.p[0]-endFrame.p[0])+
            (startFrameIn.p[1]-endFrame.p[1])*(startFrameIn.p[1]-endFrame.p[1])+
            (startFrameIn.p[2]-endFrame.p[2])*(startFrameIn.p[2]-endFrame.p[2]));
    acc=startLineVelocity*startLineVelocity/(2*sumDistance);
    timeDuration=startLineVelocity/acc;
    ratioX=(endFrame.p[0]-startFrameIn.p[0])/sumDistance;
    ratioY=(endFrame.p[1]-startFrameIn.p[1])/sumDistance;
    ratioZ=(endFrame.p[2]-startFrameIn.p[2])/sumDistance;
    isCeaseMotionFlag=true;
    isTrajFinished=false;
    currentTransitionTimeAccumued=0;
    qDebug()<<"addCeaseMotion---ratioX"<<ratioX<<ratioY<<ratioZ<<"acc"<<acc<<"vel"<<startLineVelocity
           <<"timeDuration"<<timeDuration<<"sumDistance"<<sumDistance
          <<"startFrameIn"<<startFrameIn.p[0]<<startFrameIn.p[1]<<startFrameIn.p[2]
            <<"endFrameIn"<<endFrameIn.p[0]<<endFrameIn.p[1]<<endFrameIn.p[2];

    return 1;
}

int PositionMixerManager::addStartMotion(Frame startFrameIn, Frame endFrameIn, double endLineVelocity)
{
    endLineVelocity=fabs(endLineVelocity);
    startFrame=startFrameIn;
    endFrame=endFrameIn;
    sumDistance=sqrt((startFrameIn.p[0]-endFrame.p[0])*(startFrameIn.p[0]-endFrame.p[0])+
            (startFrameIn.p[1]-endFrame.p[1])*(startFrameIn.p[1]-endFrame.p[1])+
            (startFrameIn.p[2]-endFrame.p[2])*(startFrameIn.p[2]-endFrame.p[2]));
    acc=endLineVelocity*endLineVelocity/(2*sumDistance);
    timeDuration=endLineVelocity/acc;
    ratioX=(endFrame.p[0]-startFrameIn.p[0])/sumDistance;
    ratioY=(endFrame.p[1]-startFrameIn.p[1])/sumDistance;
    ratioZ=(endFrame.p[2]-startFrameIn.p[2])/sumDistance;
    isCeaseMotionFlag=false;
    isTrajFinished=false;
    currentTransitionTimeAccumued=0;
}

int PositionMixerManager::moveOneStep_FrameCommand(Frame &frameOut,double &durationLeftOut)
{
    if(isTrajFinished)
    {
        return -1;
    }
    currentTransitionTimeAccumued+=deltaTime;
    durationLeftOut=timeDuration-currentTransitionTimeAccumued;
    if(timeDuration<currentTransitionTimeAccumued)
    {
        return 0;

    }

    double movedLength;
    if(isCeaseMotionFlag)
    {
        movedLength=sumDistance-0.5*acc*(timeDuration-currentTransitionTimeAccumued)*
                                                        (timeDuration-currentTransitionTimeAccumued);
    }
    else
    {
        movedLength=0.5*acc*(timeDuration-currentTransitionTimeAccumued)*
                                                        (timeDuration-currentTransitionTimeAccumued);
    }

    frameOut.p[0]=startFrame.p[0]+movedLength*ratioX;
    frameOut.p[1]=startFrame.p[1]+movedLength*ratioY;
    frameOut.p[2]=startFrame.p[2]+movedLength*ratioZ;
    frameOut.M=startFrame.M;
    if(0)
    {
        qDebug()<<"moveOneStep_FrameCommand---time"<<currentTransitionTimeAccumued<<"durationLeftOut"<<durationLeftOut
               <<"movedLength"<<movedLength
               <<"frameOut"<<frameOut.p[0]<<frameOut.p[1]<<frameOut.p[2];
    }
    return 1;


}
